// Field & Service Robotics
// Team 5
// Mars Manipulator
// code for the microcontroller to control grip servo and linear actuator
// wrist servo code has been commented out

#include <Servo.h>

// msg protocol:
// Av###,g###;
// A specifies actuator control
// --
// v specifies the linear actuator
// ### specifies the desired position, from 0 (fully retracted) to 255 (fully extended)
// ; specifies the end of the message
// ex. v127; would tell the actuator to go to the halfway point
// --
// g specifies the gripper servo
// ### specifies the desired position in degrees
// ; specifies the end of the message
// ex. g090; would tell the servo to go to 90 degrees

#define MSG_LENGTH 11 // length of expected msg
#define MSG_TYPE_POS 0 // the servo type specified at the 0 position of msg
#define MSG_TYPE_ACT 'A' // start of msg defining actuator
#define MSG_TYPE_GRIP 'g' 
#define MSG_TYPE_VERT 'v'
//#define MSG_TYPE_WRIST 'w'
#define MSG_END ';'


Servo gripServo; // define gripper servo object
int pGrip = 0; // position of gripper servo
int gripPin = 9; // grip Servo pin, PWM
// Servo wristServo; // define wrist servo objec

int vertPin = 3; // linear actuator pin, PWN
int pVert = 0; // position of linear actuator

boolean msgEnd = false; // boolean to control while loop, signifying end of received serial msg
int msgPtr = 0; // index of incoming character array from serial comm
char msg[MSG_LENGTH] = {'0','0','0','0','0','0','0','0','0','0'}; // incoming serial string from computer, initialized to 0's


void setup()
{
  // initialize msg array to nulls
  /*for (int i = 0; i < MSG_LENGTH; i++) 
  {
    msg[i] = 0;
  }*/
  
  Serial.begin(9600); // start serial port at 9600 bps:
  establishContact(); // send a byte to establish contact until receiver responds                                      
                        
  gripServo.attach(gripPin);  
  //resetServos();      // reset to 90 deg
  gripServo.write(90); 
  
  pinMode(vertPin,OUTPUT); // set linear actuator pin to output mode to ctrl actuator
  analogWrite(vertPin,0); // fully retract linear actuator 0 = retract to 255 = extend
  
  delay(1000);
}


void loop() 
{ 
  
  // receive input from computer
  // if we get a valid byte, read input
  // until we reach the terminating character (MSG_END), and serial bytes are available, loop to 
  // put the entire received msg into the msg character array
  while ((Serial.available() > 0) && !msgEnd && msgPtr < MSG_LENGTH) 
  {
    Serial.println("Reading msg");
    
    msg[msgPtr] = Serial.read(); // get incoming byte (a single character)

    if (msg[msgPtr] == MSG_END)
    {
      msgEnd = true;      
    } else {
      msgEnd = false;      
      msgPtr++;
    }
  }    
    
  //Serial.print("Next msg: ");
  //Serial.print(msg);
  //Serial.println();
  
  // if we have reached the end of the msg...  
  if (msgEnd == true)
  {
    
    // check to see correct msg has been received (sends the entire msg back to serial)
    Serial.println("Message received: ");
    Serial.print(msg);
    Serial.println();
 
    // now, decompose the msg into type and position
    // for the moment, there is only one type (wrist type is commented out)
    Serial.println("Parsing msg");
    
    
    char type = msg[MSG_TYPE_POS]; // determine what type of msg it is/for which servo
    //Serial.print(type);
    //Serial.println();
    
    if (type == MSG_TYPE_ACT)
    {
      // use atoi to convert the ### characters into an int
      pVert = atoi(&msg[2]);
      pGrip = atoi(&msg[7]);
      //pGrip = 10;
      //pVert = pTemp;
      
      // first handle the linear actuator
      if (pVert < 0 || pVert > 255) {
        Serial.print("ERROR: pVert outside linear actuator range! Abort vertical!");  
      } else {
        // just to check if the correct command was received/converted
        Serial.print("Writing to linear actuator: ");
        Serial.print(pVert);
        Serial.println();
        
        // write position to servo
        analogWrite(vertPin,pVert);
      }
      
      
      // next, handle the gripper servo
      if (pGrip < 0 || pGrip > 180) {
        Serial.print("ERROR: pGrip outside servo range!");  
      } else {
        // just to check if the correct command was received/converted
        Serial.print("Writing to grip servo: ");
        Serial.print(pGrip);
        Serial.println();
        
        // write position to servo
        gripServo.write(pGrip);
      }
      
    } 
   /* else if (type == MSG_TYPE_WRIST)
    {
      int pWrist = atoi(&msg[1]);
      Serial.print(pWrist);
      Serial.println();
      pWrist = map(pWrist,0,9,1,180);
      
      Serial.print("Writing to wrist servo: ");
      Serial.print(pWrist);
      Serial.println();
      
      gripServo.write(pWrist);
      
    } */
    else {
      Serial.print("ERROR: Unknown msg type - ");
      Serial.print("Message: ");
      Serial.println(msg);
    }    
    
    // otherwise msg was not recognized
  } else if (msg[0] == 0 && msg[1] == 0 && msg[2] == 0 && msg[3] == 0 && msg[4] == 0) {
    //Serial.println("Msg empty - do nothing.");
  } else {
    Serial.print("ERROR: Unkown msg - ");
    Serial.print("Message: ");
    Serial.println(msg);
  }
  
  
  // reset variables for loop
  msgEnd = false;
  msgPtr = 0;
  for (int i = 0; i < MSG_LENGTH; i++) 
  {
    msg[i] = 0;
  }
    
  delay(1000); // wait
} 

// send a byte to establish contact until receiver responds                                      
void establishContact() {
  while (Serial.available() <= 0) {
    Serial.print('A', BYTE);   // send a capital A
    delay(300);
  }
}

// reset gripServo to 90 deg position
/*void resetServos() {
     delay(100);
     gripServo.write(90); 
}*/
